// Copyright 2025 Lihan Chen
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PCD2PGM__PCD2PGM_HPP_
#define PCD2PGM__PCD2PGM_HPP_

#include <memory>
#include <string>
#include <vector>

#include "nav_msgs/msg/occupancy_grid.hpp"
#include "pcl/filters/passthrough.h"
#include "pcl/filters/voxel_grid.h"
#include "pcl/filters/statistical_outlier_removal.h"
#include "pcl/features/normal_3d.h"
#include "pcl/search/search.h"
#include "pcl/search/kdtree.h"
#include "pcl/io/pcd_io.h"
#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/point_cloud2.hpp"

namespace pcd2pgm {
class Pcd2PgmNode : public rclcpp::Node {
  public:
    explicit Pcd2PgmNode(const rclcpp::NodeOptions &options);

  private:
    void declareParameters();

    void getParameters();

    // 1. Z高度滤波
    void passThroughFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud, 
                          double thre_low, double thre_high, bool flag_in);

    // 2. 体素滤波降低计算量
    void voxelGridFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
                        double leaf_size_x, double leaf_size_y, double leaf_size_z);

    // 3. 统计离群点滤波剔除噪点
    void statisticalOutlierFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
                                 int mean_k, double std_dev_mul);

    // 4. 分块地面检测筛选地面点云并剔除
    void gridBasedGroundFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud,
                              double grid_size_x, double grid_size_y,
                              int min_points_per_grid, double height_tolerance,
                              double angle_threshold);

    // 5. PCD保存功能
    void savePointCloudToPCD(const pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,
                            const std::string &filename, const std::string &step_name);

    void
    radiusOutlierFilter(const pcl::PointCloud<pcl::PointXYZ>::Ptr &pcd_cloud0,
                        double radius, int thre_count);

    void setMapTopicMsg(const pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                        nav_msgs::msg::OccupancyGrid &msg);

    void publishCallback();

    void checkForExit();

    void applyTransform();

    float thre_z_min_;
    float thre_z_max_;
    float thre_radius_;
    bool flag_pass_through_;
    float map_resolution_;
    int thres_point_count_;
    std::string pcd_file_;
    std::string map_topic_name_;
    std::vector<double> odom_to_lidar_odom_;
    std::string map_file_path_, map_file_name_;

    // 新增的滤波参数
    // 体素滤波参数
    double voxel_leaf_size_x_, voxel_leaf_size_y_, voxel_leaf_size_z_;
    
    // 统计离群点滤波参数
    int statistical_mean_k_;
    double statistical_std_dev_mul_;
    
    // 地面移除控制参数
    bool enable_ground_removal_;
    
    // 分块地面检测参数
    double grid_size_x_;
    double grid_size_y_;
    int min_points_per_grid_;
    double ground_height_tolerance_;
    double ground_angle_threshold_;
    
    // PCD保存参数
    bool enable_pcd_save_;
    std::string pcd_save_path_;

    // 控制变量
    bool processing_completed_;

    std::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> pcd_cloud_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_pass_through_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_voxel_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_statistical_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_ground_removal_;
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_after_radius_;
    nav_msgs::msg::OccupancyGrid map_topic_msg_;

    rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_publisher_;
    rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pcd_publisher_;
    rclcpp::TimerBase::SharedPtr publish_timer_;
    rclcpp::TimerBase::SharedPtr exit_timer_;
};
} // namespace pcd2pgm

#endif // PCD2PGM__PCD2PGM_HPP_
